#include "udf_global.h"

#if ( UDF_EN_03 > 0 && !defined(UDP_SUB_PROJECT) && !defined(UDT_SUB_PROJECT) && (defined(UDF03_SUB_PROJECT) || defined(UDF_PRJ)) )
#include "udsf.h"

#include "../udsf/udsf_uart.c"
#include "../udsf/udsf_cstdlike.c"
#include "../udsf/udsf_delay.c"
#include "../udsf/udsf_memory_access.c"
#include "../UDF/rom_function2_process.c"
#include "../udsf/udsf_spi.c"
#include "../udsf/udsf_gpio.c"
#include "../udsf/udsf_protocol.c"
#include "../udsf/udsf_lpw.c"
#include "../udsf/udsf_dbscan.c"
#include "../udsf/udsf_data_send.c"
#include "../udsf/udsf_udf_switch.c"
#include "../udsf/udsf_data_acquisition.c"
#include "../udsf/udsf_cfar.c"
//#include "../udsf/udsf_bbe.c"
#include "../udsf/udsf_i2c_slave.c"


/*
xip启动，可打开udf16 自定义串口中断。默认未开启udf16，默认sys.bin内置AT接收协议。
本demo为保证OTA兼容ROM和XIP启动方式，采用系统内置中断。
*/

void UDF03_udf_handle(void){
	//udsf_spiflash_erase_sector(IIC_Para_ADDR);
/* OTA升级  */
	#if (UDK_MODE_SETS == UDK_VERSION_LITE) || (DYNALOAD_FW == 1)
	udsf_udkv2_init(); // 为兼容ROM启动，主UDF中必须首先调用此函数。UDK-SE版本无需兼容性考虑;UDK-STD版本USRBOOT内已调用此函数,无需重复调用。
	#endif
	UDK_ASSERT((p_gb_sysboot->boot_flag == APPLICATION_MODE)); // 确认程序应用模式配置正常
	#if DYNALOAD_FW == 1
	udsf_uart_init(paramFunc->baudrate,p_gb_sysboot->boot_flag);
	#define DELAY_MS (20)  // RC2412 UART-DL Tools 以启动20ms后接收到AT+OK\n来判断动态加载是否成功。动态加载用户在最终应用中可去除该部分
	#if DELAY_MS > 0
	if( p_gb_sysboot->boot_flag == 0 ){
		udsf_delay_ms_rom(DELAY_MS);
	}else{
		udsf_delay_ms_xip(DELAY_MS);
	}
	udsf_uart_send_string("AT+OK\n");
	#endif
	#elif UDK_MODE_SETS == UDK_VERSION_STD
//	udsf_uart_init(paramFunc->baudrate,p_gb_sysboot->boot_flag);
	#endif
	
//	// close wdt
//	NVIC->ICER[0] = (1 << ((uint32_t)(WDT_IRQn) & 0x1F));
//	SYSC->CLK_EN &= ~WDT_PATTERN_EN_Msk;
//	SYSC->CLK_EN |= (WDT_PATTERN_EN_Msk&(WDT_PATTERN_DISABLE<<WDT_PATTERN_EN_Pos)); // Close WDT clock 		
	
	sUDF_global.ota_status = OTA_STATUS_IDLE;
	
//	uint32_t download_addr = BBE_ABUF0_BASE;
//	uint32_t val1,val2,val3,val4;

	
	#if (DEBUG_DATA_INTERFACE == 1) || ( POINTRST_INTERFACE == 1) || (LOG_INTERFACE == 1) || ( CUSTOM_DATA_INTERFACE == 1 )
	udsf_spi_init();
	#endif
//	udsf_uart_send_string("startup test\n");
	if( paramRSV->startup_flag ){ // 全局变量初始化
		paramRSV->startup_flag = 0;
		//if( paramDataPrep->switch_flag == 0 ) // 是独立应用
		{
			sUDF_global.udf_tick=0;
			sUDF_global.udf_tick_rcd = 0;
			sUDF_global.frame_backup_index = 0;
		}
	}



	
//	/*点云数据输出*/
//	#if defined(POINTRST_ENABLE) && ( TEST_MODE == 0 )
//	#if ( POINTRST_INTERFACE == 1 )
//	FUNC_DATA_SEND_t point_cloud_output = udsf_spi_send_data;
//	#else
//	FUNC_DATA_SEND_t point_cloud_output = udsf_uart_send_data;
//	#endif	
//	#endif
	
	STRUCT_POINTRST *frameRst = (STRUCT_POINTRST *)(paramALG->framePackAddr);
	
  Write_M32(0x50010000,GPIO_PIN_1|GPIO_PIN_5);
    udsf_pinctrl_re((1 << 1), 0);
    udsf_pinctrl_ue((1 << 1), 0);
	  udsf_pinctrl_re((1 << 5), 0);
    udsf_pinctrl_ue((1 << 5), 0);
	struct{
		uint32_t sleep_time;     // 低功耗时间
//		uint32_t dp_cfg2;
		uint8_t  bb_proc;        // 基带处理标志
//		uint8_t  step_cnt;
//		uint32_t bbe_adc_addr;   // adc地址
//		uint32_t bbe_sum_addr;   // sum地址
//		uint32_t bbe_fft1d_addr; // fft1d地址
		uint8_t  mode;           // mode
	}run_param;
	uint32_t find_max;
//	run_param.step_cnt = 0;    // step cnt
	uint32_t frame_interval ; // 默认快帧
	uint16_t frame_range;
	uint16_t index_range;
	


  

 udsf_i2c_init();//i2c
 paramRSV->cnt_flag = 0;


	while(1){

#ifdef IIC_OUTPUT
//	udsf_i2c_salve_check_run_multidata();
   udsf_i2c_salve_check_run();

		
#endif		
		        
 	  	WDT->STR = 1; // feed wdt   
  
//		if ( sUDF_global.rtc_f==1)
//    {

//			for(sUDF_global.cnt=0;sUDF_global.cnt<CNT_TIME;sUDF_global.cnt++)
//			{
//			  if(GPIO0_6==1 || GPIO->INTSTR==0)
//					break;
//			}
//			if(sUDF_global.cnt==CNT_TIME)
//				sUDF_global.state=RESPOND;

//		}			
//	   sUDF_global.rtc_f=0;
//     GPIO->INTPOL = (2 << (6 << 1));
//     udsf_i2c_salve_check_run(); 
//		 sUDF_global.state = 0;    
 

		if(sUDF_global.i2c_regaa == 0xF1||sUDF_global.i2c_regaa == 0xF7)
  {//进入下载状态
//      if(sUDF_global.i2c_regaa == 0xF1)
//			sUDF_global.ota_status = OTA_STATUS_DOWNLOAD;
			sUDF_global.state=RESPOND;
      	GPIO0_5 = 0;
        GPIO0_1 = 0;
       udsf_udf_switch(3,1,0);
       break;
		}
		if( sUDF_global.ota_status == OTA_STATUS_IDLE ){
      GPIO->INTPOL = (2 << (6 << 1));			
			sUDF_global.state = 0; 
		}

//				run_param.step_cnt = 0;
				
					run_param.sleep_time     = frame_interval;//100或500
//				run_param.bbe_adc_addr   = UDFDEF_ADDR_ADCSAMP;//采样地址BUF1
//				run_param.bbe_sum_addr   = UDFDEF_ADDR_CHIRPSUM;//求和地址BUF2
//				run_param.bbe_fft1d_addr = UDFDEF_ADDR_FFT1D;//1DFFT地址BUF1
//				run_param.dp_cfg2        = UDFDEF_DP_CFG2_FRAME;//原始地址，BUF1

				run_param.bb_proc = 1;
				if( sUDF_global.udf_tick < UDFDEF_BKUP_FRAME_NUM ){ // no last frame at first-time
					run_param.bb_proc = 0;
				}    
    
//			paramDataPrep->prepCfg0 =   (UDFDEF_ANT_NUM - 1)            << 28 | // rx_num
//																	(paramRSV->chirp_sum_cnt - 1)   << 20 | // chirp_num
//																	UDFDEF_ADCSAMP_NUMLOG           << 16 | // samp_pt
//																	UDFDEF_ADCSAMP_OFFSET           <<  8 | // rma
//																	0       <<  2 | // samp_edge
//																	0       <<  0;  // adc_clk_mode
//			
//			paramDataPrep->prepCfg2 = UDFDEF_DP_CFG2_FRAME;

			/* chirp sum */
//			paramALG->alg0.p2Cfg0 = (paramALG->alg0.p2Cfg0 & 0xC00Fffff) | ((paramRSV->chirp_sum_cnt - 1) << 20);
//			paramALG->alg0.p2Cfg4 = (paramALG->alg0.p2Cfg4 & 0xffff0000) | (GET_OFFSET(run_param.bbe_adc_addr) << 0);
//			paramALG->alg0.p2Cfg8 = (paramALG->alg0.p2Cfg8 & 0xffff0000) | (GET_OFFSET(run_param.bbe_sum_addr) << 0);

			paramANA_Venus->ramp_mem10 = (paramANA_Venus->ramp_mem10 & 0xff000000) | ( paramRSV->chirp_sum_cnt<<16);
			
			
			udsf_data_acquisiton_chirpsum(0,1,0); // dac chirpsum , alg0
			#if 1 && ( ( TEST_MODE == 2 )	)	
			
			debug_frame_ctrl.frame_info.frame_class    = eFRAME_TIMING_PSEUDO_FLOATING;
			debug_frame_ctrl.frame_info.samp_num       = UDFDEF_ADCSAMP_NUM;
			debug_frame_ctrl.frame_info.samp_ant       = UDFDEF_ANT_NUM;
			debug_frame_ctrl.frame_info.samp_chirp     = UDFDEF_CHIRP_NUM;

			debug_frame_ctrl.frame_info.draw_name      = eDRAW_CHIRP_SUM;
			debug_frame_ctrl.frame_info.draw_opt       = UDFDEF_CHIRP_SUM_NUM;
			debug_frame_ctrl.frame_info.draw_name_id   = eDRAW_ANTALL;
			debug_frame_ctrl.pDataSrc                  = (uint8_t*)(run_param.bbe_sum_addr);
			
			udsf_data_send(0,&debug_frame_ctrl,-1,1,1);
      udsf_delay_ms(300);
			#endif
//			PREPDATA_HANDLE();

//			paramALG->alg1.p2Cfg4 = (paramALG->alg1.p2Cfg4 & 0xffff0000) | (GET_OFFSET(run_param.bbe_sum_addr)<< 0);
//      paramALG->alg1.p2Cfg8 = (paramALG->alg1.p2Cfg8 & 0xffff0000) | (GET_OFFSET(run_param.bbe_fft1d_addr)<<0);
      P2_FFT_HANDLE(&paramALG->alg1); //fft1d 

     
      udsf_memcpy((uint8_t*)sUDF_global.frame_bkup_pbuff[0],(uint8_t*)BBE_ABUF0_BASE,UDFDEF_RANGE_USE*UDFDEF_ANT_NUM*4);
      udsf_memcpy((uint8_t*)BBE_ABUF1_BASE,(uint8_t*)sUDF_global.frame_bkup_pbuff[0],UDFDEF_RANGE_USE*UDFDEF_ANT_NUM*4);
      
				#if 1 && ( TEST_MODE == 3 )
				for( int i=0;i<UDFDEF_ANT_NUM;i++){				
					debug_frame_ctrl.frame_info.frame_class    = eFRAME_FFT1D_PSEUDO_FLOATING;
					debug_frame_ctrl.frame_info.samp_num       = UDFDEF_RANGE_USE;
					debug_frame_ctrl.frame_info.samp_ant       = 1;
					debug_frame_ctrl.frame_info.samp_chirp     = UDFDEF_CHIRP_NUM;

					debug_frame_ctrl.frame_info.draw_name      = eDRAW_FFT1D;
					debug_frame_ctrl.frame_info.draw_opt       = 0;
					if( i == 0 ){
						debug_frame_ctrl.frame_info.draw_name_id = eDRAW_ANT1;
					}else{
						debug_frame_ctrl.frame_info.draw_name_id = eDRAW_ANT2;
					}
					
					debug_frame_ctrl.frame_info.data_len       = debug_frame_ctrl.frame_info.samp_num*debug_frame_ctrl.frame_info.samp_ant*debug_frame_ctrl.frame_info.samp_chirp*4;	
					debug_frame_ctrl.pDataSrc                  = (uint8_t*)(run_param.bbe_fft1d_addr + debug_frame_ctrl.frame_info.data_len*i);

				}
        	uint8_t frame_head[]={1,2,3,4,5,6,7,8};
		      debug_frame_ctrl.send_frame(frame_head,sizeof(frame_head));
//          debug_frame_ctrl.send_data((uint8_t*)BBE_ABUF1_BASE,UDFDEF_RANGE_USE*4*2);
          debug_frame_ctrl.send_data((uint8_t*)BBE_ABUF0_BASE,UDFDEF_RANGE_USE*4);          
          debug_frame_ctrl.send_data((uint8_t*)BBE_ABUF1_BASE,UDFDEF_RANGE_USE*4);
          udsf_delay_ms(200);
          
				#endif			
		    frameRst->targetNum = 0;
				if( run_param.bb_proc == 1 ){
//		   	run_param.step_cnt = 0;

				P2_FFT_HANDLE(&paramALG->alg2); //fft1d minus

				/* send fft1d minus rst*/
				#if 1 && ( TEST_MODE == 5 )
//					for( int i=0;i<UDFDEF_ANT_NUM;i++){
						
//					debug_frame_ctrl.frame_info.frame_class    = eFRAME_FFT1D_PSEUDO_FLOATING;
//					debug_frame_ctrl.frame_info.samp_num       = UDFDEF_RANGE_USE;
//					debug_frame_ctrl.frame_info.samp_ant       = 1;
//					debug_frame_ctrl.frame_info.samp_chirp     = UDFDEF_BKUP_FRAME_NUM;

//					debug_frame_ctrl.frame_info.draw_name      = eDRAW_FFT1D_FRAME_DIFF;
//					debug_frame_ctrl.frame_info.draw_opt       = 0;
//					if( i == 0 ){
//						debug_frame_ctrl.frame_info.draw_name_id = eDRAW_ANT1;
//						debug_frame_ctrl.pDataSrc                = (uint8_t*)UDFDEF_ADDR_FFT1D_ANT1_MINUS_OLD;
//					}else{
//						debug_frame_ctrl.frame_info.draw_name_id = eDRAW_ANT2;
//						debug_frame_ctrl.pDataSrc                = (uint8_t*)UDFDEF_ADDR_FFT1D_ANT2_MINUS_OLD;
//					}
					uint8_t frame_head[]={1,2,3,4,5,6,7,8};
		      debug_frame_ctrl.send_frame(frame_head,sizeof(frame_head));
          debug_frame_ctrl.send_data((uint8_t*)BBE_ABUF2_BASE,UDFDEF_RANGE_USE*4*UDFDEF_ANT_NUM);
          udsf_delay_ms(200);
//					udsf_data_send(0,&debug_frame_ctrl,-1,1,1);	
//				}
			#endif
				uint32_t Pow_once,Pow_twice,find_one,find_two;
				Pow_once = 0;
				Pow_twice = 0;
        uint8_t indexnum_A1,indexnum_B1,index_ave_A,index_ave_B;
        indexnum_A1 = 0;
//        indexnum_A2 = 0;
        indexnum_B1 = 0;
//        indexnum_B2 = 0;				
				index_ave_A = 0;
				index_ave_B = 0;				
				
      for(uint8_t i = 0;i < UDFDEF_CHIRP_NUM ; i++)
    {
      	for(uint8_t j = 1;j < 19 ; j++)//25cm一格，3米是12格
      	{
        	find_max = Read_M32 (BBE_ABUF2_BASE + BBE_ABS_OFFSET + j*4 + i*32*4);
			find_one = Read_M32 (BBE_ABUF2_BASE + BBE_ABS_OFFSET + j*4 -4 + i*32*4);
			find_two = Read_M32 (BBE_ABUF2_BASE + BBE_ABS_OFFSET + j*4 +4 + i*32*4);
			//距离梯度确定能量阈值
        	if(find_max > find_one && find_max > find_two && find_max > (500+sUDF_global.i2c_reg5c*100+(sUDF_global.i2c_reg67-j)*sUDF_global.i2c_reg68) )
//       	if(find_max > find_one && find_max > find_two && find_max > (1000) ) 
			{					
				if(find_max > Pow_once)
				{
          Pow_twice = Pow_once;						
					Pow_once = find_max;
					indexnum_A1 = j;
//						indexnum_A2 = i;
            		index_ave_A = (find_one*(j-1)+find_max*j+find_two*(j+1))*10/(find_max+find_one+find_two);						
				}else if(find_max > Pow_twice)
				{
					Pow_twice = find_max;						
					indexnum_B1 = j;
//						indexnum_B2 = i;						 
            		index_ave_B = (find_one*(j-1)+find_max*j+find_two*(j+1))*10/(find_max+find_one+find_two);							
				}			 
        	}
      	} 
    }	
		if(indexnum_B1<indexnum_A1 && Pow_twice>(6*Pow_once/10) && indexnum_B1!=0)
		{
		    frame_range = index_ave_B *5/2;
        index_range = indexnum_B1*23;
        find_max = Pow_twice;
		}else
		{
    		frame_range = index_ave_A *5/2;
        index_range = indexnum_A1*23;
       find_max = Pow_once;
		}
#ifndef IIC_OUTPUT
//       if(TEST_MODE == 0)
//       {

//				 
// 				udsf_uart_send_byte(0X55);
//        udsf_uart_send_byte(0XAA);
//  
//        udsf_uart_send_data((uint8_t*)&index_range,2);
//        udsf_uart_send_data((uint8_t*)&frame_range,2);
//        udsf_uart_send_data((uint8_t*)&Pow_once,4);
         
//        uint16_t Crc16 = udsf_get_crc((uint8_t*)(Bin_data),6);

//        udsf_uart_send_data((uint8_t*)&Crc16,2);          
//       } 
#endif		
       paramRSV->range_Set_outone = sUDF_global.i2c_reg10 + sUDF_global.i2c_reg11*256 + 2;
       paramRSV->range_Set_outtwo = sUDF_global.i2c_reg12 + sUDF_global.i2c_reg13*256;	
    
	  	if((index_range<=paramRSV->range_Set_outtwo) && index_range!=0)
       {
        paramRSV->two_noflag = 0;
         paramRSV->last_index_cnt ++;    
         if(paramRSV->cnt_flag<200)
          paramRSV->cnt_flag++; 
//       GPIO0_1 = 1;
       }else
			 {
        paramRSV->cnt_flag = 0;
				if(paramRSV->two_noflag < 200)
					paramRSV->two_noflag++;
       }
       
       
       if((index_range<=paramRSV->range_Set_outone) && index_range!=0)
       {
	  		paramRSV->one_noflag = 0;
			#ifndef IIC_OUTPUT		//串口调试
//			if(GPIO0_5 == 0)
//				printf("out1 = 1\r\n");
			#endif
         if(paramRSV->one_cnt_flag<paramRSV->cnt_flag)
         {
          paramRSV->one_cnt_flag = paramRSV->cnt_flag;
         }
         else
           paramRSV->one_cnt_flag++;
//			GPIO0_5 = 1;
//			GPIO0_1 = 1;				 
       }else 
       {        
        paramRSV->one_cnt_flag = 0;
         if(paramRSV->one_noflag < 200)
         {
          paramRSV->one_noflag++;
         }
       }
       if(paramRSV->one_cnt_flag > sUDF_global.i2c_reg61)
       {
         GPIO0_1 = 1;
         GPIO0_5 = 1;
       }
       if(paramRSV->cnt_flag > sUDF_global.i2c_reg61)
        GPIO0_1 = 1; 
       uint32_t noflagtime = sUDF_global.i2c_reg42 + sUDF_global.i2c_reg43*256;
       
       if(paramRSV->one_noflag>(noflagtime*100/paramRSV->frame_interval))
       GPIO0_5 = 0;
       if(paramRSV->two_noflag>(noflagtime*100/paramRSV->frame_interval))
       {
       GPIO0_1 = 0;
       paramRSV->last_index_cnt = 0;
       }
	
#ifndef IIC_OUTPUT
//       if(paramRSV->last_index_cnt == 1&& index_range!=0)
//       {
//         paramRSV->min_index_range = index_range;
//       }
//        else if(index_range < paramRSV->min_index_range && index_range!=0)
//         {
////								
//						paramRSV->min_index_range = index_range;
//					}

//         printf("Range=%dcm ,pow = %d\r\n",index_range,find_max );
//         udsf_delay_us(40);          
//        if(paramRSV->last_index_cnt >3 )
//        {
//         	printf("Range=%dcm \r\n",paramRSV->min_index_range);
//          paramRSV->last_index_cnt = 0;
//          udsf_delay_us(40);
//        } 
#endif        
//		if(frame_range != 0 && index_range<=paramRSV->range_Set_outtwo)
//		{			 
//			if(paramRSV->cnt_flag == 0)
//			{
//				paramRSV->last_index_range = index_range;			 
//			}else if(paramRSV->cnt_flag == 1)
//			{
//				paramRSV->penultimate_index_range = index_range;
//				paramRSV->min_index_range = index_range;
//			}else if((paramRSV->cnt_flag > 1) && (paramRSV->cnt_flag <= sUDF_global.i2c_reg61 + 1))
//			{
//				if(index_range < paramRSV->penultimate_index_range)
//				{
//					if(index_range < paramRSV->min_index_range)
//					{
//						paramRSV->penultimate_index_range = paramRSV->min_index_range;
//						paramRSV->min_index_range = index_range;
//					}else{
//						paramRSV->penultimate_index_range = index_range;
//					}							
//				}						
//			}
//			paramRSV->cnt_flag ++;		
//		}else if(frame_range == 0)
//		{
//			paramRSV->cnt_flag = 0;
//			paramRSV->last_index_range = 0;
//			paramRSV->min_index_range = 0;
//			paramRSV->penultimate_index_range = 0;
//		}	 
			 
			 
			 
//       if((frame_range<=paramRSV->range_Set_outtwo||index_range<=paramRSV->range_Set_outtwo) && frame_range!=0)
//       {
//    		paramRSV->two_noflag = 0;
////	        if(paramRSV->cnt_flag<200)
////          paramRSV->cnt_flag++;				
////      	GPIO0_1 = 1;
//       }
//       else if(paramRSV->two_noflag < 200)
//       {    
//			paramRSV->two_noflag++;
//       }
//			 
//		#ifndef IIC_OUTPUT		//串口调试
//			if(paramRSV->cnt_flag == sUDF_global.i2c_reg61 + 2)
//				printf("Range=%dcm \r\n",paramRSV->penultimate_index_range);
//		#endif	 
//			// printf("cnt = %d,min = %d,penultimate = %d,last = %d,index_range =%d,pow = %d  \r\n",paramRSV->cnt_flag,paramRSV->min_index_range,paramRSV->penultimate_index_range,paramRSV->last_index_range,index_range,Pow_once);
//			
//			 
//       	if(paramRSV->cnt_flag == sUDF_global.i2c_reg61 + 2)
//	   	{
//			if(((paramRSV->last_index_range - paramRSV->penultimate_index_range >= 23) && (paramRSV->last_index_range - paramRSV->penultimate_index_range <= 46) && index_range<=paramRSV->range_Set_outtwo && frame_range!=0)||(GPIO0_5 == 1))
//			{
//				#ifndef IIC_OUTPUT		//串口调试
//				if(GPIO0_1 == 0)
//					printf("out2 = 1\r\n");
//				#endif
//				GPIO0_1 = 1;							
//			}
////					if(GPIO0_5 == 1){
////						GPIO0_1 = 1;
////					}
//			paramRSV->last_index_range = paramRSV->penultimate_index_range;
//			paramRSV->min_index_range = 0;
//			paramRSV->penultimate_index_range = 0;
//			paramRSV->cnt_flag = 1;
//		}
//       	uint32_t noflagtime = sUDF_global.i2c_reg42 + sUDF_global.i2c_reg43*256;
//       	if(paramRSV->one_noflag>(noflagtime*100/paramRSV->frame_interval)){
//				#ifndef IIC_OUTPUT		//串口调试
//				if(GPIO0_5 == 1)
//					printf("out1 = 0\r\n");
//				#endif
//			GPIO0_5 = 0;
//				//printf("one_noflag = %d \r\n",paramRSV->one_noflag);
//		}
//       	if(paramRSV->two_noflag>(noflagtime*100/paramRSV->frame_interval)){
//			#ifndef IIC_OUTPUT		//串口调试
//			if(GPIO0_1 == 1)
//				printf("out2 = 0\r\n");
//			#endif
//			GPIO0_1 = 0;
//				//printf("two_noflag = %d \r\n",paramRSV->two_noflag);
//		}
		//udsf_delay_ms(5);
		}else{
//			run_param.step_cnt ++;
		} 
       	sUDF_global.udf_tick ++; 
//       uint32_t range_set;
//       if(paramRSV->range_Set_outone>paramRSV->range_Set_outtwo)
//				 range_set = (paramRSV->range_Set_outone>300)?paramRSV->range_Set_outone:300;
//			 else
//				 range_set = (paramRSV->range_Set_outtwo>300)?paramRSV->range_Set_outtwo:300;
       
       
       	paramRSV->led_timeout = sUDF_global.i2c_reg4e + sUDF_global.i2c_reg4f*256;
		uint32_t range_set;
       	if(paramRSV->range_Set_outone>paramRSV->range_Set_outtwo)
		{
			range_set = (paramRSV->range_Set_outone>300)?paramRSV->range_Set_outone:300;
		}else{
			range_set = (paramRSV->range_Set_outtwo>300)?paramRSV->range_Set_outtwo:300;
		}
       	if(frame_range <= (range_set+25) && frame_range!=0)
	   	{  
         	paramRSV->frame_interval = sUDF_global.i2c_reg3e + sUDF_global.i2c_reg3f*256;
			frame_interval = paramRSV->frame_interval;
//        if (paramRSV->chirp_sum_cnt == 1)
//        {
//         paramRSV->chirp_sum_cnt = 2;
//         sUDF_global.udf_tick = 0; 
//        }
				sUDF_global.udf_tick_rcd = sUDF_global.udf_tick + paramRSV->led_timeout; // 2s无目标退出		
		}else
		{				
			if( sUDF_global.udf_tick > sUDF_global.udf_tick_rcd )
			{
					// 切换到低帧率
				paramRSV->sleep_time2 = sUDF_global.i2c_reg31 + sUDF_global.i2c_reg32*256;
				frame_interval = paramRSV->sleep_time2;
				GPIO0_5 = 0;
				GPIO0_1 = 0;
//					paramRSV->cnt_flag = 0;
//					paramRSV->last_index_range = 0;
//					paramRSV->min_index_range = 0;
//					paramRSV->penultimate_index_range = 0;
          
//        if (paramRSV->chirp_sum_cnt == 2)
//        {
//         paramRSV->chirp_sum_cnt = 1;
//         sUDF_global.udf_tick = 0; 
//        }

					// led off

			}
		}  
#ifndef IIC_OUTPUT
     if(GPIO0_5 == 1)
       printf("GPIO0_5 = 1");
     if(GPIO0_1 == 1)
       printf("GPIO0_1 = 1");
#endif    
//		frame_interval = 300;
		run_param.sleep_time  = frame_interval;

		udsf_rtc_set(0,(run_param.sleep_time-4)*34);
//		udsf_rtc_set(1,(run_param.sleep_time*34));
//	if(sUDF_global.ota_status == 0)
    {
		/*sleep enter -> lrc/128 clk*/
		udsf_lpw_enter();

		/*fake sleep*/
//		udsf_rtc_get_block(0); // wait rtc1
  	udsf_iic_wakeup(0);    // wait iic and rtc0
//    测试整机休眠功耗		
//		while(1){
//			WDT->STR = 1;
//		}
		/*sleep exit -> lrc/1 clk*/
		udsf_lpw_exit();

		/* wait delay for osc ready */
//		udsf_rtc_get_block(1); // wait rtc1

		/*full-speed run*/
		udsf_full_speed();
	}
		if ( sUDF_global.rtc_f==1)
    {

			for(sUDF_global.cnt=0;sUDF_global.cnt<CNT_TIME;sUDF_global.cnt++)
			{
			  if(GPIO0_6==1 || GPIO->INTSTR==0)
					
					break;
			}
			if(sUDF_global.cnt==CNT_TIME)
				sUDF_global.state=RESPOND;

		}		    
		
		
	}
}
#endif
